/*
The MIT License
Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE

This file is part of embeddedRTPS.

Author: i11 - Embedded Software, RWTH Aachen University
*/

#include <stdio.h>
#include "rtps/discovery/ParticipantProxyData.h"

//using rtps::ParticipantProxyData;

/* extern */

/* Internal */

/* func */
/* ???? */
ParticipantProxyData *rtps_ParticipantProxyData_Init_Default(ParticipantProxyData *pin) {
  
  pin->m_protocolVersion = PROTOCOLVERSION;
  pin->m_guid.prefix = GUIDPREFIX_UNKNOWN;
  pin->m_guid.entityId = ENTITYID_UNKNOWN;
  pin->m_vendorId = VENDOR_UNKNOWN;
  pin->m_expectsInlineQos = false;
  pin->m_manualLivelinessCount.value = 1;
  pin->m_leaseDuration = SPDP_DEFAULT_REMOTE_LEASE_DURATION;
#if defined(unix) || defined(__unix__)
  /* ???? */
  /* second */
  //std::chrono::time_point<std::chrono::high_resolution_clock>
  //    m_lastLivelinessReceivedTimestamp;
  gettimeofday(&pin->m_lastLivelinessReceivedTimestamp, NULL);
#else
  pin->m_lastLivelinessReceivedTickCount = 0;
#endif

  rtps_ParticipantProxyData_onAliveSignal(pin);

  return pin;
}

//ParticipantProxyData::ParticipantProxyData(Guid_t guid) : m_guid(guid) {}
ParticipantProxyData *rtps_ParticipantProxyData_Init(ParticipantProxyData *pin, Guid_t guid){
  //memcpy(pin, &guid, sizeof(Guid_t));
  pin->m_guid = guid;
  return pin;
}

// Needs to be in header because they are marked with inline
bool rtps_ParticipantProxyData_hasParticipantWriter(ParticipantProxyData *pin) {
  return (pin->m_availableBuiltInEndpoints &
          DISC_BUILTIN_ENDPOINT_PARTICIPANT_ANNOUNCER) == 1;
}

bool rtps_ParticipantProxyData_hasParticipantReader(ParticipantProxyData *pin) {
  return (pin->m_availableBuiltInEndpoints &
          DISC_BUILTIN_ENDPOINT_PARTICIPANT_DETECTOR) != 0;
}

bool rtps_ParticipantProxyData_hasPublicationWriter(ParticipantProxyData *pin) {
  return (pin->m_availableBuiltInEndpoints &
          DISC_BUILTIN_ENDPOINT_PUBLICATION_ANNOUNCER) != 0;
}

bool rtps_ParticipantProxyData_hasPublicationReader(ParticipantProxyData *pin) {
  return (pin->m_availableBuiltInEndpoints &
          DISC_BUILTIN_ENDPOINT_PUBLICATION_DETECTOR) != 0;
}

bool rtps_ParticipantProxyData_hasSubscriptionWriter(ParticipantProxyData *pin) {
  return (pin->m_availableBuiltInEndpoints &
          DISC_BUILTIN_ENDPOINT_SUBSCRIPTION_ANNOUNCER) != 0;
}

bool rtps_ParticipantProxyData_hasSubscriptionReader(ParticipantProxyData *pin) {
  return (pin->m_availableBuiltInEndpoints &
          DISC_BUILTIN_ENDPOINT_SUBSCRIPTION_DETECTOR) != 0;
}

/* ???? */
void rtps_ParticipantProxyData_onAliveSignal(ParticipantProxyData *pin) {
#if defined(unix) || defined(__unix__)
  /* second */
  //m_lastLivelinessReceivedTimestamp = std::chrono::high_resolution_clock::now();
  gettimeofday(&pin->m_lastLivelinessReceivedTimestamp, NULL);
#else
  m_lastLivelinessReceivedTickCount = xTaskGetTickCount();
#endif
}

/* ???? */
uint32_t rtps_ParticipantProxyData_getAliveSignalAgeInMilliseconds(ParticipantProxyData *pin) {
#if defined(unix) || defined(__unix__)
  //auto now = std::chrono::high_resolution_clock::now();
  //std::chrono::duration<double, std::milli> duration =
  //    now - m_lastLivelinessReceivedTimestamp;
  struct timeval now;
  long duration;
  gettimeofday(&now, NULL);
  if (now.tv_usec > pin->m_lastLivelinessReceivedTimestamp.tv_usec)
  {
    /* ms */
    duration = (now.tv_usec - pin->m_lastLivelinessReceivedTimestamp.tv_usec) / 1000;
  }
  else
  {
    /* ms */
    duration = (((now.tv_sec - pin->m_lastLivelinessReceivedTimestamp.tv_sec) * 1000000)
                   + now.tv_usec - pin->m_lastLivelinessReceivedTimestamp.tv_usec) / 1000;
  }

  return (uint32_t)duration;
#else
  return (xTaskGetTickCount() - m_lastLivelinessReceivedTickCount) *
         (1000 / configTICK_RATE_HZ);
#endif
}

/*
 *  Returns true if last heartbeat within lease duration, else false
 */
bool rtps_ParticipantProxyData_isAlive(ParticipantProxyData *pin) {
  uint32_t lease_in_ms =
      pin->m_leaseDuration.seconds * 1000 + pin->m_leaseDuration.fraction * 1e-6;

  uint32_t max_lease_in_ms =
      SPDP_MAX_REMOTE_LEASE_DURATION.seconds * 1000 +
      SPDP_MAX_REMOTE_LEASE_DURATION.fraction * 1e-6;

  uint32_t heatbeat_age_in_ms = 
      rtps_ParticipantProxyData_getAliveSignalAgeInMilliseconds(pin);

  uint32_t min;

  if (lease_in_ms < max_lease_in_ms)
    min = lease_in_ms;
  else
    min = max_lease_in_ms;
  /* ???? */
  //if (heatbeat_age_in_ms > std::min(lease_in_ms, lease_in_ms)) {
  if (heatbeat_age_in_ms > min) {
    return false;
  }
  return true;
}

void rtps_ParticipantProxyData_reset(ParticipantProxyData *pin) {
  pin->m_guid.prefix = GUIDPREFIX_UNKNOWN;
  pin->m_guid.entityId = ENTITYID_UNKNOWN;
  pin->m_manualLivelinessCount.value = 1;
  pin->m_expectsInlineQos = false;
  for (int i = 0; i < SPDP_MAX_NUM_LOCATORS; ++i) {
    rtps_Locator_setInvalid((Locator *)(void *)&pin->m_metatrafficUnicastLocatorList[i]);
    rtps_Locator_setInvalid((Locator *)(void *)&pin->m_metatrafficMulticastLocatorList[i]);
    rtps_Locator_setInvalid((Locator *)(void *)&pin->m_defaultUnicastLocatorList[i]);
    rtps_Locator_setInvalid((Locator *)(void *)&pin->m_defaultMulticastLocatorList[i]);
  }
}

bool rtps_ParticipantProxyData_readFromUcdrBuffer(ParticipantProxyData *pin, ucdrBuffer *buffer) {
  rtps_ParticipantProxyData_reset(pin);
  ParameterId pid;
  uint16_t length;
  while (ucdr_buffer_remaining(buffer) >= 4) {
    ucdr_deserialize_uint16_t(buffer, (uint16_t *)(void *)(&pid));

    ucdr_deserialize_uint16_t(buffer, &length);
    if (ucdr_buffer_remaining(buffer) < length) {
      return false;
    }

    switch (pid) {
    case PID_KEY_HASH: {
      // TODO
      break;
    }
    case PID_PROTOCOL_VERSION: {
      ucdr_deserialize_uint8_t(buffer, &pin->m_protocolVersion.major);
      if (pin->m_protocolVersion.major < PROTOCOLVERSION.major) {
        return false;
      } else {
        ucdr_deserialize_uint8_t(buffer, &pin->m_protocolVersion.minor);
      }
      break;
    }
    case PID_VENDORID: {
      ucdr_deserialize_array_uint8_t(buffer, pin->m_vendorId.vendorId,
                                     sizeof(pin->m_vendorId.vendorId));
      break;
    }

    case PID_EXPECTS_INLINE_QOS: {
      ucdr_deserialize_bool(buffer, &pin->m_expectsInlineQos);
      break;
    }
    case PID_PARTICIPANT_GUID: {
      ucdr_deserialize_array_uint8_t(buffer, pin->m_guid.prefix.id,
                                     sizeof(pin->m_guid.prefix.id));
      ucdr_deserialize_array_uint8_t(buffer, pin->m_guid.entityId.entityKey,
                                     sizeof(pin->m_guid.entityId.entityKey));
      ucdr_deserialize_uint8_t(
          buffer, (uint8_t *)(void *)(&pin->m_guid.entityId.entityKind));
      break;
    }
    case PID_METATRAFFIC_MULTICAST_LOCATOR: {
      if (!rtps_ParticipantProxyData_readLocatorIntoList(
        buffer, pin->m_metatrafficMulticastLocatorList)) {
        return false;
      }
      break;
    }
    case PID_METATRAFFIC_UNICAST_LOCATOR: {
      if (!rtps_ParticipantProxyData_readLocatorIntoList(
        buffer, pin->m_metatrafficUnicastLocatorList)) {
        return false;
      }
      break;
    }
    case PID_DEFAULT_UNICAST_LOCATOR: {
      if (!rtps_ParticipantProxyData_readLocatorIntoList(
        buffer, pin->m_defaultUnicastLocatorList)) {
        return false;
      }
      break;
    }
    case PID_DEFAULT_MULTICAST_LOCATOR: {
      if (!rtps_ParticipantProxyData_readLocatorIntoList(
        buffer, pin->m_defaultMulticastLocatorList)) {
        return false;
      }
      break;
    }
    case PID_PARTICIPANT_LEASE_DURATION: {
      ucdr_deserialize_int32_t(buffer, &pin->m_leaseDuration.seconds);
      ucdr_deserialize_uint32_t(buffer, &pin->m_leaseDuration.fraction);
      break;
    }
    case PID_BUILTIN_ENDPOINT_SET: {
      ucdr_deserialize_uint32_t(buffer, &pin->m_availableBuiltInEndpoints);
      break;
    }
    case PID_ENTITY_NAME: {
      // TODO
      buffer->iterator += length;
      buffer->last_data_size = 1;
      break;
    }
    case PID_PROPERTY_LIST: {
      // TODO
      buffer->iterator += length;
      buffer->last_data_size = 1;
      break;
    }
    case PID_USER_DATA: {
      // TODO
      buffer->iterator += length;
      buffer->last_data_size = 1;
      break;
    }
    case PID_PAD: {
      buffer->iterator += length;
      buffer->last_data_size = 1;
      break;
    }
    case PID_SENTINEL: {
      return true;
    }
    default: { 
      return false;
    }
    }
    // Parameter lists are 4-byte aligned
    uint32_t alignment = ucdr_buffer_alignment(buffer, 4);
    buffer->iterator += alignment;
    buffer->last_data_size = 4;
  }
  return true;
}

//Locator list[SPDP_MAX_NUM_LOCATORS];

bool rtps_ParticipantProxyData_readLocatorIntoList(
    ucdrBuffer *buffer,
    Locator *list) {
  int i;

  for (i=0; i < SPDP_MAX_NUM_LOCATORS; i++){
    if (!rtps_Locator_isValid(list)) {
      rtps_Locator_readFromUcdrBuffer(list, buffer);
      return true;
    }
    list++;
  }

  return false;
}
